{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "#coding:utf-8\n",
    "import cv2\n",
    "import numpy as np\n",
    "import glob"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "####   相机标定"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "# 找棋盘格角点\n",
    "# 阈值：角点精准化迭代过程的终止条件\n",
    "criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)\n",
    "#棋盘格模板规格\n",
    "w = 6\n",
    "h = 7\n",
    "# 世界坐标系中的棋盘格点,例如(0,0,0), (1,0,0), (2,0,0) ....,(8,5,0)，去掉Z坐标，记为二维矩阵\n",
    "objp = np.zeros((w*h,3), np.float32)\n",
    "objp[:,:2] = np.mgrid[0:w,0:h].T.reshape(-1,2)\n",
    "#print(objp)\n",
    "# 储存棋盘格角点的世界坐标和图像坐标对\n",
    "objpoints = [] # 在世界坐标系中的三维点\n",
    "imgpoints = [] # 在图像平面的二维点\n",
    "# print(np.mgrid[0:w,0:h])\n",
    "# print(np.mgrid[0:w,0:h].T)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "8\n"
     ]
    }
   ],
   "source": [
    "#读取棋盘格图片\n",
    "# import os\n",
    "# os.mkdir('image3')\n",
    "\n",
    "images = glob.glob('image\\*.jpg')\n",
    "i=0\n",
    "print(len(images))\n",
    "for fname in images:\n",
    "    img = cv2.imread(fname)\n",
    "    gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)\n",
    "    # 找到棋盘格角点(角点坐标)\n",
    "    ret, corners = cv2.findChessboardCorners(gray, (w,h),None)\n",
    "    #print(corners)\n",
    "    # 如果找到足够点对，将其存储起来\n",
    "    if ret == True:\n",
    "        # 执行亚像素级角点检测\n",
    "        cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria)\n",
    "        objpoints.append(objp)\n",
    "        imgpoints.append(corners)\n",
    "        # 将角点在图像上显示\n",
    "        #print(corners)\n",
    "        cv2.drawChessboardCorners(img, (w,h), corners, ret)\n",
    "        cv2.imshow('findCorners',img)\n",
    "        cv2.waitKey(1)\n",
    "        cv2.imwrite('image3\\%d.jpg'%(i), img)\n",
    "        i=i+1\n",
    "cv2.destroyAllWindows()\n",
    "#help(cv2.cornerSubPix)\n",
    "# # 去畸变\n",
    "# img2 = cv2.imread('calib/00169.png')\n",
    "# h,  w = img2.shape[:2]\n",
    "# newcameramtx, roi=cv2.getOptimalNewCameraMatrix(mtx,dist,(w,h),0,(w,h)) # 自由比例参数\n",
    "# dst = cv2.undistort(img2, mtx, dist, None, newcameramtx)\n",
    "# # 根据前面ROI区域裁剪图片\n",
    "# #x,y,w,h = roi\n",
    "# #dst = dst[y:y+h, x:x+w]\n",
    "# cv2.imwrite('calibresult.png',dst)\n",
    "\n",
    "# # 反投影误差\n",
    "# total_error = 0\n",
    "# for i in xrange(len(objpoints)):\n",
    "#     imgpoints2, _ = cv2.projectPoints(objpoints[i], rvecs[i], tvecs[i], mtx, dist)\n",
    "#     error = cv2.norm(imgpoints[i],imgpoints2, cv2.NORM_L2)/len(imgpoints2)\n",
    "#     total_error += error\n",
    "# print \"total error: \", total_error/len(objpoints)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "ret: 0.9225111687037113\n",
      "内参数矩阵mtx:\n",
      " [[1.11732472e+03 0.00000000e+00 5.46508645e+02]\n",
      " [0.00000000e+00 1.15545645e+03 7.44689319e+02]\n",
      " [0.00000000e+00 0.00000000e+00 1.00000000e+00]]\n",
      "畸变系数dist:\n",
      " [[ 0.11862898 -0.37391003  0.00461     0.00501408  0.24464077]]\n",
      "旋转矩阵rvecs:\n",
      " [array([[ 0.39461212],\n",
      "       [ 0.5770537 ],\n",
      "       [-1.59454718]]), array([[ 0.53770903],\n",
      "       [ 0.37631004],\n",
      "       [-1.45980441]]), array([[ 0.49547514],\n",
      "       [ 0.53992861],\n",
      "       [-1.48643064]]), array([[-0.06179691],\n",
      "       [-0.02113152],\n",
      "       [-1.53432546]]), array([[ 0.91567898],\n",
      "       [-0.04791366],\n",
      "       [-1.3849702 ]]), array([[ 0.00920016],\n",
      "       [ 0.94149446],\n",
      "       [-1.4266042 ]]), array([[ 0.56048831],\n",
      "       [ 0.54134455],\n",
      "       [-2.08183831]]), array([[ 0.48115345],\n",
      "       [ 0.54398673],\n",
      "       [-1.07368457]])]\n",
      "平移矩阵tvecs:\n",
      " [array([[-2.27041674],\n",
      "       [ 2.40368069],\n",
      "       [19.83100747]]), array([[-4.88065072],\n",
      "       [ 2.07949786],\n",
      "       [18.20303453]]), array([[-2.87337466],\n",
      "       [ 2.77885543],\n",
      "       [19.70701279]]), array([[-2.98795405],\n",
      "       [ 2.86763813],\n",
      "       [14.76376743]]), array([[-4.35869602],\n",
      "       [ 0.62897025],\n",
      "       [11.95249289]]), array([[-2.76702799],\n",
      "       [ 0.79045853],\n",
      "       [13.93625625]]), array([[-0.83760049],\n",
      "       [ 2.64431999],\n",
      "       [14.84206729]]), array([[-3.56652198e+00],\n",
      "       [ 1.19736687e-02],\n",
      "       [ 1.30993873e+01]])]\n"
     ]
    }
   ],
   "source": [
    "#mtx，相机内参；dist，畸变系数；revcs，旋转矩阵；tvecs，平移矩阵。\n",
    "ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, None,)\n",
    "\n",
    "print(\"ret:\", ret)\n",
    "\n",
    "print(\"内参数矩阵mtx:\\n\", mtx)  # 内参数矩阵\n",
    "\n",
    "print(\"畸变系数dist:\\n\", dist)\n",
    "# 畸变系数   distortion cofficients = (k_1,k_2,p_1,p_2,k_3)\n",
    "print(\"旋转矩阵rvecs:\\n\", rvecs)\n",
    "# 旋转向量  # 外参数\n",
    "print(\"平移矩阵tvecs:\\n\", tvecs)\n",
    "# 平移向量  # 外参数"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "id=0:(435,750)\n",
      "id=1:(434,735)\n",
      "id=2:(433,719)\n",
      "id=3:(431,703)\n",
      "id=4:(430,686)\n",
      "id=5:(428,670)\n",
      "id=6:(451,749)\n",
      "id=7:(450,733)\n",
      "id=8:(448,717)\n",
      "id=9:(447,701)\n",
      "id=10:(446,685)\n",
      "id=11:(444,668)\n",
      "id=12:(467,748)\n",
      "id=13:(465,732)\n",
      "id=14:(464,716)\n",
      "id=15:(463,700)\n",
      "id=16:(462,684)\n",
      "id=17:(461,667)\n",
      "id=18:(483,746)\n",
      "id=19:(481,731)\n",
      "id=20:(480,715)\n",
      "id=21:(479,699)\n",
      "id=22:(478,682)\n",
      "id=23:(477,666)\n",
      "id=24:(498,745)\n",
      "id=25:(497,729)\n",
      "id=26:(496,713)\n",
      "id=27:(495,697)\n",
      "id=28:(494,681)\n",
      "id=29:(492,665)\n",
      "id=30:(514,744)\n",
      "id=31:(513,728)\n",
      "id=32:(512,712)\n",
      "id=33:(511,696)\n",
      "id=34:(509,680)\n",
      "id=35:(508,664)\n",
      "id=36:(529,742)\n",
      "id=37:(528,727)\n",
      "id=38:(527,711)\n",
      "id=39:(526,695)\n",
      "id=40:(525,679)\n",
      "id=41:(524,662)\n"
     ]
    }
   ],
   "source": [
    "# import os\n",
    "# print(os.getcwd())\n",
    "# path=os.getcwd()+'\\one.jpg'\n",
    "# print(path)\n",
    "# fname =os.getcwd()+'\\0.jpg'\n",
    "img1 = cv2.imread('1m.jpg')  # source image\n",
    "gray1 = cv2.cvtColor(img1, cv2.COLOR_BGR2GRAY)  # 转灰度\n",
    "\n",
    "# 寻找角点，存入corners，ret是找到角点的flag\n",
    "ret, corners = cv2.findChessboardCorners(gray1, (6, 7), None)\n",
    "criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)\n",
    "corners2 = cv2.cornerSubPix(gray1, corners, (11, 11), (-1, -1), criteria)\n",
    "#print(corners2)\n",
    "count = 0\n",
    "for corner in corners2:\n",
    "    a, b = corner[0]\n",
    "    a = int(a)\n",
    "    b = int(b)\n",
    "\n",
    "    cv2.putText(img1, '(%s)' % (count), (a, b), cv2.FONT_HERSHEY_COMPLEX, 0.5, (0, 255, 0), 1)\n",
    "    print('id={}:({},{})'.format(count, a, b))\n",
    "    count += 1\n",
    "\n",
    "    img = cv2.drawChessboardCorners(img1, (w, h), corners2, ret)\n",
    "    cv2.imshow('img', img1)\n",
    "    cv2.waitKey(50)\n",
    "    cv2.imwrite('1m_id.jpg', img1)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "####   位姿估计"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "found True\n",
      "rvec [[-0.08581038]\n",
      " [ 0.07381543]\n",
      " [ 0.02681929]]\n",
      "tvec [[  0.62972643]\n",
      " [-10.04868262]\n",
      " [ 24.43342993]]\n",
      "[[ 0.99691948 -0.02992234  0.07249968]\n",
      " [ 0.02359535  0.99596321  0.08660571]\n",
      " [-0.07479846 -0.08462826  0.99360115]]\n",
      "[[  1.43689866  12.09471984 -23.45246576]]\n",
      "==============================\n",
      "-4.86831952739087 4.289642548109118 1.3558384312734182\n",
      "camPos: [25.18905718] [-401.94730498] [977.33719725]\n"
     ]
    }
   ],
   "source": [
    "import math\n",
    "# 1m.jpg 中： 单位：格子（一个格子40mm）       对应像素坐标和id\n",
    "# 左上：0，0                id = 5:(578,266)\n",
    "# 左下：0，5              id = 0:(568,502)\n",
    "# 右上：6，0              id = 41:(858,265)\n",
    "# 右下：6，5            id = 36:(857,500)\n",
    "\n",
    "# camX = 240\n",
    "# camY = 315\n",
    "\n",
    "object_3d_points = np.array(([0, 0, 0],\n",
    "                             [0, 5, 0],\n",
    "                             [6, 0, 0],\n",
    "                             [6, 5, 0]), dtype=np.double)\n",
    "object_2d_point = np.array(([578, 266],\n",
    "                            [568, 502],\n",
    "                            [858, 265],\n",
    "                            [857, 500]), dtype=np.double)\n",
    "\n",
    "dist_coefs = np.array([0.11862898, -0.37391001, 0.00461, 0.00501408, 0.24464072],\n",
    "                      dtype=np.double)\n",
    "camera_matrix = np.array((\n",
    "    [1.11732472e+03, 0.00000000e+00, 5.46508645e+02],\n",
    "    [0.00000000e+00, 1.15545645e+03, 7.44689319e+02],\n",
    "    [0.00000000e+00, 0.00000000e+00, 1.00000000e+00]), dtype=np.double)\n",
    "# 求解相机位姿\n",
    "found, rvec, tvec = cv2.solvePnP(object_3d_points, object_2d_point, camera_matrix, dist_coefs)\n",
    "\n",
    "print('found', found)\n",
    "print('rvec', rvec)\n",
    "print('tvec', tvec)\n",
    "\n",
    "rotM = cv2.Rodrigues(rvec)[0]\n",
    "print(rotM)\n",
    "camera_postion = -np.matrix(rotM).T * np.matrix(tvec)\n",
    "print(camera_postion.T)\n",
    "# 验证根据博客http://www.cnblogs.com/singlex/p/pose_estimation_1.html提供方法求解相机位姿\n",
    "# 计算相机坐标系的三轴旋转欧拉角，旋转后可以转出世界坐标系。旋转顺序z,y,x\n",
    "thetaZ = math.atan2(rotM[1, 0], rotM[0, 0]) * 180.0 / math.pi\n",
    "thetaY = math.atan2(-1.0 * rotM[2, 0], math.sqrt(rotM[2, 1] ** 2 + rotM[2, 2] ** 2)) * 180.0 / math.pi\n",
    "thetaX = math.atan2(rotM[2, 1], rotM[2, 2]) * 180.0 / math.pi\n",
    "# 相机坐标系下值\n",
    "x = tvec[0] * 40\n",
    "y = tvec[1] * 40\n",
    "z = tvec[2] * 40\n",
    "print('=' * 30)\n",
    "print(thetaX, thetaY, thetaZ)\n",
    "print(\"camPos:\", x, y, z)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.7.1"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
